/**
 * TransNode.cpp
 * Coyright (c) 2009 Xiaofeng Gu. All Rights Reserved.
 * Author : Xiaofeng Gu
 *
 * Creation Data : 2009/06/11
 */

#include "stdafx.h"
#include "TransNode.h"
#include <math.h>
#include "Global.h"

#ifdef _DEBUG
#define new DEBUG_NEW
#endif


CTransNode::CTransNode(CKnot* pParentKnot)
: m_pParent(NULL),
  m_pParentKnot(pParentKnot)
{
	m_pRotAxis = &(m_oDiff.deltaDirection.selfRotAxis);
	m_pAngle = &(m_oDiff.deltaDirection.angle);
	m_pScrewAngle = &(m_oDiff.deltaNormal.angle);
	m_pFactor = &(m_oDiff.deltaSize.factor);
}

CTransNode::~CTransNode()
{
}

bool CTransNode::OnRead(CXMLParser* pParser)
{
	//pParser->FindChildNode(NODE_TRANSNODE);
	m_strNodeName = pParser->GetName();
	m_bActive = pParser->GetActive();
		m_oBone.OnRead(pParser);
		{
			while(pParser->FindChildNode(NODE_TRANSNODE))
			{
				CTransNode* pChildNode = new CTransNode(m_pParentKnot);
				m_oChildren.push_back(pChildNode);
					pChildNode->OnRead(pParser);
				pChildNode->SetParent(this);
				pParser->EndXMLNode();
			}
		}

	return __super::OnRead(pParser);
}

bool CTransNode::OnWrite(CXMLParser* pParser)
{
	pParser->AddChildNode(NODE_TRANSNODE);
	pParser->AddName(m_strNodeName);
	pParser->AddActive(m_bActive);
		m_oBone.OnWrite(pParser);
		{
			for (int i = 0; i < (int)(m_oChildren.size()); i ++)
			{
				CTransNode* pChild = m_oChildren[i];
				pChild->OnWrite(pParser);
			}
		}
	pParser->EndXMLNode();

	return __super::OnWrite(pParser);
}

bool CTransNode::OnSelect(double Ray[6])
{
	double x0, x1, x2, y0, y1, y2, z0, z1, z2;
	x1 = Ray[0];
	y1 = Ray[1];
	z1 = Ray[2];
	x2 = Ray[3];
	y2 = Ray[4];
	z2 = Ray[5];
	x0 = (m_oBone[0][0] + m_oBone[1][0]) / 2;
	y0 = (m_oBone[0][1] + m_oBone[1][1]) / 2;
	z0 = (m_oBone[0][2] + m_oBone[1][2]) / 2;
	double A = x1 - x0; double B = x2 - x1;
	double C = y1 - y0; double D = y2 - y1;
	double E = z1 - z0; double F = z2 - z1;
	double R = ((m_oBone[1] - m_oBone[0]).Length()) / 2;
	double G = A * A + C * C + E * E - R * R;
	double H = 2 * (A * B + C * D + E * F);
	double I = B * B + D * D + F * F;
	if (H * H - 4 * I * G > 0)
		return true;
	else
		return false;
}

bool CTransNode::RotateTo(CVec& ToVec)
{
	CVec direction = m_oBone.Direction();
	
	CVec temp1 = direction;
	temp1.Identify();
	CVec temp2 = ToVec;
	temp2.Identify();
	if (temp1.Equal(temp2[0], temp2[1], temp2[2]))
		return true;

	double dAngle = direction.CrossAngle(ToVec);
	if (fabs(dAngle) < DELTA) return true;
	CVec crossVec = direction ^ ToVec;
	return Rotate(crossVec, dAngle);
}

bool CTransNode::Rotate(CVec& Ref, double dAngle)
{
	m_oBone.Rotate(Ref, dAngle);
	m_pRotAxis->RotateBy(Ref, dAngle);

	for (int i = 0; i < (int)(m_oChildren.size()); i ++)
	{
		CTransNode* pChild = m_oChildren[i];
		if (!pChild->Rotate(m_oBone[0], Ref, dAngle))
			return false;
	}

	return true;
}

bool CTransNode::Rotate(CJoint& oJoint, CVec& Ref, double dAngle)
{
	m_oBone.Rotate(oJoint, Ref, dAngle);
	m_pRotAxis->RotateBy(Ref, dAngle);

	for (int i = 0; i < (int)(m_oChildren.size()); i ++)
	{
		CTransNode* pChild = m_oChildren[i];
		if (!pChild->Rotate(oJoint, Ref, dAngle))
			return false;
	}

	return true;
}

bool CTransNode::Screw(double dAngle)
{
	CVec ref = m_oBone[1] - m_oBone[0];
	m_oBone.Screw(dAngle);

	for (int i = 0; i < (int)(m_oChildren.size()); i ++)
	{
		CTransNode* pChild = m_oChildren[i];
		if (!pChild->Rotate(ref, dAngle))
			return false;
	}

	return true;
}

bool CTransNode::Move(CVec& Ref)
{
	m_lastMovement = Ref;

	if (!m_oBone.Move(Ref))
			return false;

	for (int i = 0; i < (int)(m_oChildren.size()); i ++)
	{
		CTransNode* pChild = m_oChildren[i];
		if (!pChild->Move(Ref))
			return false;
	}

	return true;
}

bool CTransNode::MoveTo(CVec& Pt)
{
	CVec beforeMove = CVec(m_oBone[0][0], m_oBone[0][1], m_oBone[0][2]);
	CVec moveVec = Pt - beforeMove;

	if (fabs(moveVec.Length()) > DELTA)
	{
		Move(moveVec);
	}

	return true;
}

bool CTransNode::Zoom(double K)
{
	CVec V = m_oBone[1] - m_oBone[0];
	V *= K;
	m_oBone[1] = m_oBone[0] + V;

	for (int i = 0; i < (int)(m_oChildren.size()); i ++)
	{
		CTransNode* pChild = m_oChildren[i];
		if (!pChild->Move(V))
			return false;
	}

	return true;
}

NodeDifference* CTransNode::operator-(CTransNode& node)
{
	// calculate direction difference
	{
		CVec V1 = m_oBone[1] - m_oBone[0];
		CVec V2 = (*node.GetBone())[1] - (*node.GetBone())[0];
		CVec V3 = V1 ^ V2;
		double dRotAngle = 0;
		if (V3.Length() < DELTA)
		{
			if (m_pParent != NULL)
			{
				CVec parent = (*m_pParent->GetBone())[1] - (*m_pParent->GetBone())[0];
				if (parent * V1 < DELTA)
				{
					V3 = parent;
				}
				else
				{
					V3 = V1 ^parent;
				}
			}
			else
			{
				CVec vec(0, 0, 1);
				if (vec * V1 < DELTA)
				{
					V3 = vec;
				}
				else
				{
					V3 = V1 ^ vec;
				}
			}
		}

		dRotAngle = acos(V1 * V2 / (sqrt(V1*V1) * sqrt(V2*V2)));
		*m_pAngle = dRotAngle;
		*m_pRotAxis = V3;

		if (fabs(dRotAngle) > DELTA)
		{
			Rotate(V3, dRotAngle);
		}
	}

	// calculate normal difference
	{
		CVec nV1 = m_oBone.GetNormal();
		CVec nV2 = (*node.GetBone()).GetNormal();

		double dScrewAngle = 0;
		if (fabs(sqrt(nV1 * nV1)) < DELTA
			|| fabs(sqrt(nV2 * nV2)) < DELTA)
		{
			dScrewAngle = 0;
		}
		else
		{
			dScrewAngle = acos(nV1 * nV2 / (sqrt(nV1*nV1) * sqrt(nV2*nV2)));
		}
		*m_pScrewAngle = dScrewAngle;

		if (fabs(dScrewAngle) > DELTA)
		{
			Screw(dScrewAngle);
		}
	}

	// calculate size difference
	{
		CVec V1 = m_oBone[1] - m_oBone[0];
		CVec V2 = (*node.GetBone())[1] - (*node.GetBone())[0];
		double dFactor = sqrt(V2*V2) / sqrt(V1*V1);
		*m_pFactor = dFactor;

		if (fabs(dFactor - 1) > DELTA)
		{
			Zoom(dFactor);
		}
	}

	return &m_oDiff;
}

void CTransNode::operator++()
{
	if (fabs(*m_pAngle) > DELTA)
	{
		Rotate(*m_pRotAxis, *m_pAngle);
	}

	if (fabs(*m_pScrewAngle) > DELTA)
	{
		Screw(*m_pScrewAngle);
	}

	if (fabs((*m_pFactor) - 1) > DELTA)
	{
		Zoom((*m_pFactor));
	}
}

void CTransNode::operator--()
{
	if (fabs(*m_pAngle) > DELTA)
	{
		Rotate(*m_pRotAxis, - (*m_pAngle));
	}

	if (fabs(*m_pScrewAngle) > DELTA)
	{
		Screw(-(*m_pScrewAngle));
	}

	if (fabs((*m_pFactor) - 1) > DELTA)
	{
		Zoom(1.0/(*m_pFactor));
	}
}

void CTransNode::Render()
{
	if (!IsVisibility()) return;

	if (IsHighLight())
	{
		CSKGlobal::GetGraphPlotter()->SetLineWidth(5.0);
		CSKGlobal::GetGraphPlotter()->SetColor(255.0f, 0.0f, 0.0f);
	}
	else
	{
		CSKGlobal::GetGraphPlotter()->SetColor(255.0f,255.0f,255.0f);
	}

	Streams oStreams;
	oStreams.push_back(m_oBone[0][0]);
	oStreams.push_back(m_oBone[0][1]);
	oStreams.push_back(m_oBone[0][2]);
	oStreams.push_back(m_oBone[1][0]);
	oStreams.push_back(m_oBone[1][1]);
	oStreams.push_back(m_oBone[1][2]);
	CSKGlobal::GetGraphPlotter()->Plot(CGraphPlotter::eLines, oStreams);
	CSKGlobal::GetGraphPlotter()->SetLineWidth(1.0);
}

CGBox& CTransNode::GetRangeBox()
{
	double min[3];
	double max[3];

	if (m_oBone[0][0] < m_oBone[1][0])
	{
		min[0] = m_oBone[0][0];
		max[0] = m_oBone[1][0];
	}
	else
	{
		min[0] = m_oBone[1][0];
		max[0] = m_oBone[0][0];
	}

	if (m_oBone[0][1] < m_oBone[1][1])
	{
		min[1] = m_oBone[0][1];
		max[1] = m_oBone[1][1];
	}
	else
	{
		min[1] = m_oBone[1][1];
		max[1] = m_oBone[0][1];
	}

	if (m_oBone[0][2] < m_oBone[1][2])
	{
		min[2] = m_oBone[0][2];
		max[2] = m_oBone[1][2];
	}
	else
	{
		min[2] = m_oBone[1][2];
		max[2] = m_oBone[0][2];
	}

	m_oRangeBox = CGBox(min[0], min[1], min[2],
		max[0], max[1], max[2]);
	return m_oRangeBox;
}

CTransNode* CTransNode::Copy(CKnot* pParentKnot)
{
	CTransNode* pNew = new CTransNode(pParentKnot);

	// native information
	pNew->m_oBone = m_oBone;
	pNew->m_strNodeName = m_strNodeName;
	pNew->m_bVisibility = m_bVisibility;
	pNew->m_bHighLight = m_bHighLight;
	pNew->m_oRangeBox = m_oRangeBox;
	pNew->m_bExpanded = m_bExpanded;

	return pNew;
}

void CTransNode::Back()
{
	m_oBackBone = m_oBone;
}

void CTransNode::Rollback()
{
	m_oBone = m_oBackBone;
}